#include "Joystick.h"
#include <math.h>

/*
 *    操作杆中位默认AD值：2.5v = 1861
 *    操作杆全行程范围：372~3351 (0.5~4.5v)
 *    操作杆有效范围: 70% (中位向上或向下，最大最小死区各15%)
 */

// #define JOYSTICK_BASE_ADC_VALUE 1861
// #define JOYSTICK_ADC_MIN 372
// #define JOYSTICK_ADC_MAX 3351
#define JOYSTICK_BASE_ADC_VALUE 383
#define JOYSTICK_ADC_MIN 383
#define JOYSTICK_ADC_MAX 3368
#define JOYSTICK_VALID_PERCENT 70

#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE) && HAL_JOYSTICK_DUAL_CHANNEL_TYPE == HAL_JOYSTICK_SIGNAL_TYPE_CROSS

#define JOYSTICK_CHANNEL_SUM (JOYSTICK_BASE_ADC_VALUE * 2) // 双轴和，5V
#define JOYSTICK_CHANNEL_OFFSET 223                        // 双轴和允许误差，±0.3V波动

#endif

#if !defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE) || HAL_JOYSTICK_DUAL_CHANNEL_TYPE == HAL_JOYSTICK_SIGNAL_TYPE_CROSS
#define JOYSTICK_ADC_RANGE ((JOYSTICK_ADC_MAX - JOYSTICK_ADC_MIN) / 2)
#else
#define JOYSTICK_ADC_RANGE (JOYSTICK_ADC_MAX - JOYSTICK_ADC_MIN)
#endif
#define JOYSTICK_ADC_EFFECT_RANGE ((JOYSTICK_ADC_RANGE * JOYSTICK_VALID_PERCENT) / 100) // 有效值范围
#define JOYSTICK_DEAD_ZONE_VAL ((JOYSTICK_ADC_RANGE - JOYSTICK_ADC_EFFECT_RANGE) / 2)   // 死区值

static bool g_joystickInitReady = false;
static JoystickErrorReport_t gErrorReport;

static void updateError(SysJoystickChannel_t ch, JoystickErrorID_t error)
{
    if (error != JOYSTICK_ERR_NONE)
    {
        if (gErrorReport.error == JOYSTICK_ERR_NONE)
        {
            gErrorReport.ch = ch;
            gErrorReport.error = error;
        }
    }
    else
    {
        if (ch == gErrorReport.ch)
        {
            gErrorReport.error = error;
        }
    }
}

uint16_t JoystickGetRawValue(SysJoystickChannel_t ch, SysJoystickDirect_t dir)
{
    uint8_t chA;
    uint16_t adcValA;
#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE)
    uint8_t chB;
    uint16_t adcValB;
    uint32_t sum;
#endif

    switch (ch)
    {
    case SYS_JOYSTICK_CH_DBSJ: // 大臂升降
        chA = HAL_ADC_JOYSTICK_DBSJ_A;
#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE)
        chB = HAL_ADC_JOYSTICK_DBSJ_B;
#endif
        break;
    case SYS_JOYSTICK_CH_DBSS: // 大臂伸缩
        chA = HAL_ADC_JOYSTICK_DBSS_A;
#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE)
        chB = HAL_ADC_JOYSTICK_DBSS_B;
#endif
        break;
    case SYS_JOYSTICK_CH_ZTXZ: // 转台旋转
        chA = HAL_ADC_JOYSTICK_ZTXZ_A;
#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE)
        chB = HAL_ADC_JOYSTICK_ZTXZ_B;
#endif
        break;
    case SYS_JOYSTICK_CH_DGSJ: // 吊钩升降
        chA = HAL_ADC_JOYSTICK_DGSJ_A;
#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE)
        chB = HAL_ADC_JOYSTICK_DGSJ_B;
#endif
        break;
    default:
        break;
    }

    adcValA = HalADCGetChannelValue(chA);
#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE)
    adcValB = HalADCGetChannelValue(chB);

#if (HAL_JOYSTICK_DUAL_CHANNEL_TYPE == HAL_JOYSTICK_SIGNAL_TYPE_CROSS)
    sum = adcValA + adcValB;
    if (sum > JOYSTICK_CHANNEL_SUM + JOYSTICK_CHANNEL_OFFSET ||
        sum < JOYSTICK_CHANNEL_SUM - JOYSTICK_CHANNEL_OFFSET) // AB双轴曲线交叉方向变化，两者和始终为5V
    {
        updateError(ch, JOYSTICK_ERR_DISCONNECTED);
        return JOYSTICK_BASE_ADC_VALUE;
    }
#else
    if (adcValA > JOYSTICK_DEAD_ZONE_VAL + JOYSTICK_ADC_MIN &&
        adcValB > JOYSTICK_DEAD_ZONE_VAL + JOYSTICK_ADC_MIN)
    {
        updateError(ch, JOYSTICK_ERR_DISCONNECTED);
        return JOYSTICK_BASE_ADC_VALUE;
    }

    // return (dir == SYS_JOYSTICK_DIR_UP ? adcValA : adcValB);
#endif

#endif
    updateError(ch, JOYSTICK_ERR_NONE);
    
#if defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE) && (HAL_JOYSTICK_DUAL_CHANNEL_TYPE == HAL_JOYSTICK_SIGNAL_TYPE_SEPARATE)
    return (dir == SYS_JOYSTICK_DIR_UP ? adcValA : adcValB);
#else
    return adcValA;
#endif
}

unsigned char JoystickGetPercent(SysJoystickChannel_t ch, SysJoystickDirect_t dir)
{
    int16_t adc = (int16_t)JoystickGetRawValue(ch, dir);
    uint8_t percent = 0;
    uint32_t adcAbs;
    int16_t diff;

    if (adc < (JOYSTICK_ADC_MIN - 100) || adc > (JOYSTICK_ADC_MAX + 300))
    {
        updateError(ch, JOYSTICK_ERR_DISCONNECTED);
        return 0; // error
    }
    else
    {
        updateError(ch, JOYSTICK_ERR_NONE);
    }

    // range = (JOYSTICK_ADC_MAX - JOYSTICK_ADC_MIN) / 2; //摇杆单向ADC范围
    // effectRange = range * JOYSTICK_VALID_PERCENT / 100; //有效范围（除死区以外）
    // offset = (range - effectRange) / 2; //死区范围
    diff = adc - JOYSTICK_BASE_ADC_VALUE; //(JOYSTICK_ADC_MIN + JOYSTICK_ADC_RANGE);

    adcAbs = abs(diff);
    if (adcAbs > JOYSTICK_DEAD_ZONE_VAL)
    {
#if !defined(HAL_JOYSTICK_DUAL_CHANNEL_ENABLE) || HAL_JOYSTICK_DUAL_CHANNEL_TYPE == HAL_JOYSTICK_SIGNAL_TYPE_CROSS
        if (dir == (SYS_JOYSTICK_DIR_UP ^ (diff < 0))) // 获取方向和数值符号一致
#endif
        {
            if (adcAbs >= JOYSTICK_ADC_EFFECT_RANGE + JOYSTICK_DEAD_ZONE_VAL)
            {
                percent = 100;
            }
            else
            {
                percent = (adcAbs - JOYSTICK_DEAD_ZONE_VAL) * 100 / JOYSTICK_ADC_EFFECT_RANGE;
            }
        }
    }

    return percent;
}

bool JoystickIsReady(void)
{
    return g_joystickInitReady;
}

void JoystickReset(void)
{
    g_joystickInitReady = false;
}

JoystickErrorReport_t *JoystickErrorCheck(void)
{
    return &gErrorReport;
}

void JoystickInitialize(void)
{
    g_joystickInitReady = false;
}

void JoystickPoll(void)
{
    static SysTime_t lastCheckTime = 0;
    SysJoystickChannel_t ch;

    if (!g_joystickInitReady && SysTimeHasPast(lastCheckTime, 50))
    {
        g_joystickInitReady = true;
        for (ch = (SysJoystickChannel_t)0; ch < SYS_JOYSTICK_CH_COUNT; ch++)
        {

            if (JoystickGetPercent(ch, SYS_JOYSTICK_DIR_UP) != 0 ||
                JoystickGetPercent(ch, SYS_JOYSTICK_DIR_DOWN) != 0)
            {
                g_joystickInitReady = false;
                updateError(ch, JOYSTICK_ERR_NOT_ZEROED);
                break;
            }
            else
            {
                updateError(ch, JOYSTICK_ERR_NONE);
            }
        }
        lastCheckTime = SysTime();
    }
}
